//
// Created by Pulsar on 2021/8/23.
//
#include <iostream>
#include <signal.h>
#include <stdlib.h>
#include <unistd.h>
#include <mraa/common.hpp>
#include <mraa/gpio.hpp>
#include <mraa/pwm.hpp>


volatile sig_atomic_t flag = 1;

void sig_handler(int signum) {
    if (signum == SIGINT) {
        std::cout << "Exiting..." << std::endl;
        flag = 0;
    }
}

void dir_pin(mraa::Gpio *gpio) {
    mraa::Result status = gpio->dir(mraa::DIR_OUT);
    if (status != mraa::SUCCESS) {
        printError(status);
    }
}

void set_value(mraa::Gpio *gpio, int value) {
    mraa::Result status = gpio->write(value);
    if (status != mraa::SUCCESS) {
        printError(status);
    }
}

int dir_1 = 1;

int main(int argc, char **argv) {


    signal(SIGINT, sig_handler);
    int weel_1 = 40;
    int weel_2 = 36;
    int weel_3 = 38;


    mraa::Pwm pwm_1(32);
    mraa::Pwm pwm_2(33);
    mraa::Pwm pwm_3(16);

    mraa::Gpio gpio_weel_1(weel_1);
    mraa::Gpio gpio_weel_2(weel_2);
    mraa::Gpio gpio_weel_3(weel_3);

    dir_pin(&gpio_weel_1);
    dir_pin(&gpio_weel_2);
    dir_pin(&gpio_weel_3);

    pwm_1.enable(true);
    pwm_2.enable(true);
    pwm_3.enable(true);
    float HZ = 600;
    bool direction = false;
    int step = 1;

    set_value(&gpio_weel_1, 0);
    set_value(&gpio_weel_2, 0);
    set_value(&gpio_weel_3, 0);
    while (flag) {
        float freq = (1 / HZ) * 1000000;
        float width = freq / 2;
        std::cout << "HZ:" << HZ << " Width:" << width << " Freq:" << freq << std::endl;
        if (HZ < 500 or HZ > 8000) {
            direction = !direction;
        }
        if (direction) {
            HZ -= step;
        } else {
            HZ += step;
        }

        pwm_1.period_us((int) freq);
        pwm_2.period_us((int) freq);
        pwm_3.period_us((int) freq);

        pwm_1.pulsewidth_us((int) width);
        pwm_2.pulsewidth_us((int) width);
        pwm_3.pulsewidth_us((int) width);
        usleep(50000);

        dir_1 = !dir_1;

//                set_value(&gpio_weel_1, dir_1);
//                set_value(&gpio_weel_2, dir_1);
//                set_value(&gpio_weel_3, dir_1);
        //        sleep(2);



    }
    pwm_1.enable(false);
    pwm_2.enable(false);
    pwm_3.enable(false);
    return 0;
}

